#pragma config(Sensor, in1,    TubeSensor,          sensorLineFollower)
#pragma config(Sensor, in5,    PotArm,              sensorPotentiometer)
#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Motor,  port1,           ArmLL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           DriveLF,       tmotorNormal, openLoop)
#pragma config(Motor,  port3,           DriveLB,       tmotorNormal, openLoop)
#pragma config(Motor,  port4,           DriveRF,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           DriveRB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           ArmLU,         tmotorNormal, openLoop)
#pragma config(Motor,  port7,           ArmRU,         tmotorNormal, openLoop)
#pragma config(Motor,  port8,           collector1,    tmotorNormal, openLoop)
#pragma config(Motor,  port9,           collector2,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          ArmRL,         tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "alpha_lib.c" //Main Funtion Library
//#include "PIDController.c"
//--/ Drive /-------------------------------------------------------/
int DriveMode;
bool filter;

//--/ PID /-------------------------------------------------------/
PIDController arm;
int startpoint = 0;
int goal_value = startpoint;
int change = -1000;
int k_P = 10;
int k_I = 0;
int k_D = 0;

//--/ Arm Position /----------------------------------------------/
int low_descore;
int low_lock;
int high_descore;
int high_lock;


void pre_auton() {
  //--/ Drive /-------------------------------------------------------/
  DriveMode=0;
  //--/ Arm Points /-----------------------/
  //goal_value = startpoint + change;
  startpoint/*arm_grounded*/ = SensorValue[PotArm];  // sets ground point           (0 inches)
  low_descore = 1592;//startpoint + 1556 - 1236;          // sets low descore arm point  (4.5 inches)
  low_lock = 917;//startpoint + 2265 - 1236;             //...lowgoal                   (15 inches)
  high_descore = 917;//startpoint + 1879- 1247;          //...high descore              (x inches)
  high_lock = 556;//tartpoint + 2599 - 1247;            // ...high goal                (18.5 inches)
  goal_value = startpoint;
  //--/ PID /------------------------------/
  init(arm);

  setPIDs(arm, k_P, k_I, k_D);
  setSetpoint(arm, goal_value);

  enable(arm);

  arm.k_P = 5;
}

task autonomous() {
}

task usercontrol() {
    pre_auton();

    while (true) {
/*******DRIVE******************************************************************/
        /* Original drive code, basic, no input scaling
          motor[DriveLB] = motor[DriveLF] = checkDeadZone(vexRT[Ch3]);
          motor[DriveRB] = motor[DriveRF] = checkDeadZone(vexRT[Ch2]);*/

        //Allows buttons to change drive mode (scaling function)
        if(vexRT[Btn8U] == 1){// no scaling
                DriveMode = 0;
                filter=false;
            }
        if(vexRT[Btn8R] == 1){//Squaring function, concave up
                DriveMode = 1;
                filter=false;
            }
        if(vexRT[Btn8D] == 1){//arcsin function, concave up
                DriveMode = 2;
                filter=false;
            }
        if(vexRT[Btn8L] == 1){//Sets to arctan(x^2) function, concave down
                filter=true;
            }
//parameters (left input,right input,scaling (t/f),rate filter,(t/f),DriveMode)
//rate filter disabled by default as constant has not been determined*/
        driveTank(vexRT[Ch3], vexRT[Ch2], true, filter,DriveMode);
/*******ARM********************************************************************/
    if ((vexRT[Btn5D] - vexRT[Btn5U])!=0){
      setArmSpeed((vexRT[Btn5D] - vexRT[Btn5U]) * FULL);
      disable(arm);
    } else {
      setArmSpeed(calculatePID(arm, SensorValue[PotArm]));
    }

    //PID Controlled Presets
    if(vexRT[Btn7U] == 1) {
      setSetPoint(arm, high_lock);
      enable(arm);
    } else if (vexRT[Btn7D] == 1) {
      setSetPoint(arm, low_descore);
      enable(arm);
    } else if(vexRT[Btn7L] == 1) {
      disable(arm);
    }
/*******COLLECTOR**************************************************************/
        motor[collector1] = motor[collector2] = (vexRT[Btn6U] - vexRT[Btn6D]) * FULL;
    }
}
